#include "matrix.h"
#include <cmath>

#define TO_RAD(x) ((x * M_PI)/ 180.0)

Matrix::Matrix()
{
}

Matrix::Matrix(const Matrix &m) {
    this->copy(m);
}

Matrix::Matrix(const Vector &col1, const Vector &col2, const Vector &col3, const Vector &col4)
{
    (*this)(0, 0) = col1(0);
    (*this)(1, 0) = col1(1);
    (*this)(2, 0) = col1(2);
    (*this)(3, 0) = col1(3);

    (*this)(0, 1) = col2(0);
    (*this)(1, 1) = col2(1);
    (*this)(2, 1) = col2(2);
    (*this)(3, 1) = col2(3);

    (*this)(0, 2) = col3(0);
    (*this)(1, 2) = col3(1);
    (*this)(2, 2) = col3(2);
    (*this)(3, 2) = col3(3);

    (*this)(0, 3) = col4(0);
    (*this)(1, 3) = col4(1);
    (*this)(2, 3) = col4(2);
    (*this)(3, 3) = 1.0;
}

Matrix::Matrix(const Vector &col1, const Vector &col2, const Vector &col3)
{
    (*this)(0, 0) = col1(0);
    (*this)(0, 1) = col1(1);
    (*this)(0, 2) = col1(2);
    (*this)(0, 3) = col1(3);

    (*this)(1, 0) = col2(0);
    (*this)(1, 1) = col2(1);
    (*this)(1, 2) = col2(2);
    (*this)(1, 3) = col2(3);

    (*this)(2, 0) = col3(0);
    (*this)(2, 1) = col3(1);
    (*this)(2, 2) = col3(2);
    (*this)(2, 3) = col3(3);

    (*this)(3, 0) = 0.0;
    (*this)(3, 1) = 0.0;
    (*this)(3, 2) = 0.0;
    (*this)(3, 3) = 1.0;
}

Matrix::~Matrix()
{
}

Matrix& Matrix::operator=(const Matrix &b) {
    if( this != &b ) {
        this->copy(b);
    }

    return (*this);
}

void Matrix::assignRow(int row, const Vector &v)
{
    elements_[row][0] = v(0);
    elements_[row][1] = v(1);
    elements_[row][2] = v(2);
    elements_[row][3] = v(3);
//    (*this)(row, 0) = v(0);
//    (*this)(row, 1) = v(1);
//    (*this)(row, 2) = v(2);
//    (*this)(row, 3) = v(3);
}

void Matrix::assignRow(int row, const Point &p)
{
    (*this)(row, 0) = p(0);
    (*this)(row, 1) = p(1);
    (*this)(row, 2) = p(2);
    (*this)(row, 3) = p(3);
}

void Matrix::assignCol(int col, const Vector &v)
{
    (*this)(0, col) = v(0);
    (*this)(1, col) = v(1);
    (*this)(2, col) = v(2);
    (*this)(3, col) = v(3);
}

void Matrix::assignCol(int col, const Point &p)
{
    (*this)(0, col) = p(0);
    (*this)(1, col) = p(1);
    (*this)(2, col) = p(2);
    (*this)(3, col) = p(3);
}

Vector Matrix::row(int row) const
{
    Vector v;
    v(0) = (*this)(row, 0);
    v(1) = (*this)(row, 1);
    v(2) = (*this)(row, 2);
    v(3) = (*this)(row, 3);

    return v;
}

Vector Matrix::col(int col) const
{
    Vector v;
    v(0) = (*this)(0, col);
    v(1) = (*this)(1, col);
    v(2) = (*this)(2, col);
    v(3) = (*this)(3, col);

    return v;
}

Matrix Matrix::transpose() const
{
    Matrix t;

    for(int i = 0 ; i < 4 ; i++) {
        for(int j = 0 ; j < 4 ; j++) {
            t(i, j) = (*this)(j, i);
        }
    }

    return t;
}

void Matrix::toOpenGLMatrix(double *glMatrixBuffer) const
{
    for(int i = 0 ; i < 4 ; i++) {
        for(int j = 0 ; j < 4 ; j++) {
            *(glMatrixBuffer + 4*j + i) = (*this)(i, j);
        }
    }
}

const Matrix Matrix::identity()
{
    Matrix id;

    for(int i = 0 ; i < 4 ; i++) {
        for(int j = 0 ; j < 4 ; j++) {
            if( i == j )
                id(i, j) = 1;
            else
                id(i, j) = 0;
        }
    }

    return id;
}

const Matrix Matrix::translationMatrix(double x, double y, double z)
{
    return Matrix::translationMatrix(Vector(x, y, z));
}

const Matrix Matrix::translationMatrix(const Vector &t)
{
    Matrix tm = Matrix::identity();
    for(int i = 0 ; i < 3 ; i++)
        tm(i, 3) = t(i);

    return tm;
}

const Matrix Matrix::xRotationMatrix(double angle)
{
    Matrix r = Matrix::identity();
    double rad_angle = TO_RAD(angle);
    double c = cos(rad_angle);
    double s = sin(rad_angle);

    r(1, 1) = c;
    r(2, 2) = c;
    r(1, 2) = -s;
    r(2, 1) = s;

    return r;
}

const Matrix Matrix::yRotationMatrix(double angle)
{
    Matrix r = Matrix::identity();
    double rad_angle = TO_RAD(angle);
    double c = cos(rad_angle);
    double s = sin(rad_angle);

    r(0, 0) = c;
    r(2, 2) = c;
    r(0, 2) = s;
    r(2, 0) = -s;

    return r;
}

const Matrix Matrix::zRotationMatrix(double angle)
{
    Matrix r = Matrix::identity();
    double rad_angle = TO_RAD(angle);
    double c = cos(rad_angle);
    double s = sin(rad_angle);

    r(0, 0) = c;
    r(1, 1) = c;
    r(0, 1) = -s;
    r(1, 0) = s;

    return r;
}

const Matrix Matrix::rotationMatrix(double angle, double x, double y, double z)
{
    Vector normalizedAxis(x, y, z);
    normalizedAxis.normalize();

    return Matrix::rotationMatrix(angle, normalizedAxis);
}

const Matrix Matrix::rotationMatrix(double angle, const Vector &axis)
{
    Matrix Rx = Matrix::identity();
    Matrix Ry = Matrix::identity();
    double d = sqrt(axis(1)*axis(1) + axis(2)*axis(2));

    Rx(1, 1) = Rx(2, 2) = axis(2) / d;
    Rx(1, 2) = -axis(1) / d;
    Rx(2, 1) = -Rx(1, 2);

    Ry(0, 0) = Ry(2, 2) = d;
    Ry(0, 2) = -axis(0);
    Ry(2, 0) = axis(0);

    return Rx.transpose()*Ry.transpose()*Matrix::zRotationMatrix(angle)*Ry*Rx;
}

const Matrix Matrix::scalingMatrix(double sx, double sy, double sz)
{
    Matrix tm = Matrix::identity();
    tm(0, 0) = sx;
    tm(1, 1) = sy;
    tm(2, 2) = sz;

    return tm;
}

const Matrix Matrix::coordChangeMatrix(const GlFrame &dst)
{
    Vector lastCol(-(dst.i()*dst.source()), -(dst.j()*dst.source()), -(dst.k()*dst.source()));
    Vector c0(dst.i()(0), dst.j()(0), dst.k()(0));
    Vector c1(dst.i()(1), dst.j()(1), dst.k()(1));
    Vector c2(dst.i()(2), dst.j()(2), dst.k()(2));

    return Matrix(c0, c1, c2, lastCol);
}

void Matrix::copy(const Matrix &b)
{
    for(int i = 0 ; i < 4 ; i++) {
        for(int j = 0 ; j < 4 ; j++)
            elements_[i][j] = b(i, j);
    }
}

Matrix operator+(const Matrix &left, const Matrix &right)
{
    Matrix c;
    for(int i = 0 ; i < 4 ; i++) {
        for(int j = 0 ; j < 4 ; j++)
            c(i,j) = left(i, j) + right(i, j);
    }

    return c;
}

Matrix operator-(const Matrix &left, const Matrix &right)
{
    Matrix c;
    for(int i = 0 ; i < 4 ; i++) {
        for(int j = 0 ; j < 4 ; j++)
            c(i,j) = left(i, j) - right(i, j);
    }

    return c;
}

Matrix operator*(const Matrix &left, const Matrix &right)
{
    Matrix c;
    for(int i = 0 ; i < 4 ; i++) {
        for(int j = 0 ; j < 4 ; j++) {
            double dotprod = 0.0;
            for(int k = 0 ; k < 4 ; k++)
                dotprod += left(i, k) * right(k, j);
            c(i, j) = dotprod;
        }
    }

    return c;
}

Vector operator*(const Matrix &M, const Vector &v) {
    Vector c;
    for(int i = 0 ; i < 4 ; i++) {
        double dotprod = 0;
        for(int j = 0 ; j < 4 ; j++) {
            dotprod += M(i, j) * v(j);
        }

        c(i) = dotprod;
    }

    return c;
}

Point operator*(const Matrix &M, const Point &p) {
    Point c;
    for(int i = 0 ; i < 4 ; i++) {
        double dotprod = 0;
        for(int j = 0 ; j < 4 ; j++) {
            dotprod += M(i, j) * p(j);
        }

        c(i) = dotprod;
    }

    return c;
}
